Chapter  9 

Reconfigurable  Guidance  System 


This  chapter  presents  a  guidance  algorithm  for  a  UAV.  It  combines  a  nonlin¬ 
ear  lateral  guidance  control  law,  originally  designed  for  UAVs  tracking  circles 
for  mid-air  rendezvous,  with  a  new  simple  adaptive  path-planning  algorithm. 
Preflight  path  planning  consists  only  of  storing  a  few  waypoints  guiding  the 
aircraft  to  its  targets.  The  chapter  presents  an  efficient  way  to  model  no-fly 
zones  (NFZ),  to  generate  a  path  in  real  time  to  avoid  known  or  “pop-up” 
obstacles,  and  to  reconfigure  the  flight  path  in  the  event  of  reduced  aircraft 
performance.  Simulation  results  show  the  good  performance  of  this  recon¬ 
figurable  guidance  system  which,  moreover,  is  computationally  efficient  [1, 

i 


9.1  Introduction 

Over  the  last  two  decades,  many  path-planning  algorithms  have  been  inves¬ 
tigated,  especially  for  ground  robots,  for  a  single  UAV,  and  more  recently 
for  a  formation  of  UAVs.  Among  the  methods  used  in  path  planning,  we  can 
mention  the  PRM  method  [3],  which  explores  all  the  possible  paths  within 
the  space  surrounding  the  vehicle  and  finally  selects  the  lowest  cost  route. 
However,  the  computational  load  makes  the  PRM  method  impractical  for 
real-time  path  planning  in  small  UAVs.  An  extension  to  the  PRM  method  has 
recently  been  presented  in  [4].  It  is  called  modified  rapidly- exploring  random 
trees ,  which  is  capable  of  efficiently  searching  for  feasible  paths  in  the  space 
while  taking  into  account  constraints  from  the  vehicle  performance.  However, 
efforts  are  still  going  on  to  implement  an  on-the-fly  path-replanning  system 
as  pop-up  obstacles  are  discovered  or  when  the  performance  of  the  vehicle 
degrades. 

There  are  other  methods  based  on  potential  field  functions.  However,  the 
primitive  forms  of  potential  field  functions  present  some  difficulties  when 
choosing  an  appropriate  potential  function,  and  the  algorithm  may  be  stuck 
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at  some  local  minimum  [5].  Since  then,  a  whole  family  of  potential  field 
methods  with  superior  performance  has  been  developed.  They  are  known 
as  navigation  functions  [6,  7].  Other  path-planning  techniques  are  based  on 
optimization  methods,  such  as  mixed  integer  linear  programming  or  MPC 
techniques  [8],  which  still  involve  intensive  computations. 

In  this  chapter,  we  present  a  reconfigurable  guidance  algorithm  for  a  UAV. 
It  newly  combines  the  lateral  guidance  control  law  from  [9]  and  [10],  originally 
designed  for  UAVs  tracking  circles  for  mid-air  rendezvous,  with  a  new,  sim¬ 
ple  adaptive  path-planning  algorithm,  which  takes  advantage  of  the  curve 
path-following  property  of  the  above-mentioned  lateral  guidance  law.  This 
path-planning  method  generates  on-line  a  flight  path  based  on  predefined 
waypoints,  takes  into  account  the  aircraft  performance,  avoids  known  or  ap¬ 
pearing  obstacles,  is  simple  to  implement,  and  requires  low  computational 
power. 
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Fig.  9.1  Guidance  system  inputs  and  outputs 


As  shown  in  Fig.  9.1,  the  guidance  system  needs  six  inputs.  The  first  input 
concerns  the  aircraft’s  current  ground  position  (xjv,  Xe,  Xd)-  The  second 
input  is  the  aircraft’s  ground  velocity  (Vn,  Ve,  Vd),  The  mission  of  the 
aircraft  is  defined  by  a  list  of  waypoints  through  which  the  aircraft  is  to  fly. 
Furthermore,  if  in  the  area  of  the  flight  operation  some  obstacles  or  NFZ  are 
known  in  advance  or  appear  during  the  flight,  their  location  and  dimensions 
can  be  specified  to  the  guidance  system  via  the  fourth  input.  A  constraint 
on  the  maximum  bank  angle  4>max, left/right  is  given  to  the  guidance  system. 
Finally,  the  parameter  Trou  is  provided  as  an  estimate  of  the  maximum  time 
needed  to  bank  the  aircraft  to  <f>max-  Note  that  the  last  four  inputs  can 
be  changed  dynamically,  and  the  first  two  inputs  are  obviously  constantly 
updated. 

The  outputs  of  the  guidance  system  are  the  commanded  bank  angle  <j>c, 
whose  value  is  computed  by  the  lateral  guidance  system  detailed  in  Sects.  9.2 
and  9.3.  The  altitude  command  signal  hc  is  computed  by  the  altitude  guidance 
system  described  in  Sect.  9.4.  Finally,  the  commanded  aircraft  velocity  Vc  can 
be  adaptively  controlled  by  the  guidance  system  in  order  to  efficiently  avoid 
obstacles  and  reach  the  goals  of  the  mission  optimally.  Note,  however,  that 
in  this  chapter  the  velocity  command  Vc  is  kept  to  a  constant  value. 


9.2  Lateral  Guidance  System 

9.2  Lateral  Guidance  System 


203 


9.2.1  Lateral  Guidance  Control  Law  for  Trajectory 
Tracking 

Consider  Fig.  9.2,  where  an  aircraft  has  to  be  guided  to  track  the  desired 
path.  From  the  current  location  of  the  aircraft  O,  we  can  draw  a  circular 
arc  that  intersects  the  desired  path  at  a  “reference  point”  P,  where  R  is  the 
radius  of  the  circle-arc  OP,  Li  is  the  segment  that  joins  the  center  of  the 
aircraft  O  to  the  reference  point  P,  and  r]  is  the  angle  between  the  aircraft’s 
velocity  vector  and  the  line  L\. 


C 

Fig.  9.2  Guidance  law  geometry 


The  lateral  acceleration  required  to  bring  the  aircraft  to  the  reference  point 
following  the  arc  of  a  circle  is 


V2 

ai  =  -jj-  ,  (9-1) 

where  the  ground  speed  of  the  aircraft  (taken  in  the  local  navigation  frame)  is 
Vn  =  sJVff  +  V2.  Let  us  express  R  in  terms  of  the  distance  L\  and  the  angle 
r/.  The  triangle  (OCP)  is  isosceles  in  C,  therefore,  we  have  L\  =  2i?sin  6,  or 
also  Li  =  2/?,  cos  7.  Moreover,  the  angle  7  =  f  —  7,  and  consequently,  the 
length  Li  can  be  expressed  in  terms  of  the  angle  t]  as  follows: 


L\  =  2i?sinry  , 
2smry 


The  lateral  acceleration  in  (9.1)  can  now  be  written 


(9.2) 

(9.3) 
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a;  =  sin  77  .  (9.4) 

In  turn,  the  lateral  acceleration  cq  is  converted  to  a  bank  angle  command, 
0com  ~  ai/g  (see  Appendix  D.2). 


9.2.2  Advantages  and  Properties  of  the  Method 

9. 2. 2.1  Remarkable  Performance  for  Curve  Path  Following 

This  control  law  is  remarkable  in  the  sense  that  it  is  particularly  suited  to  fly 
circles.  Indeed,  if  the  aircraft  is  following  a  desired  circular  path,  then  the  ac¬ 
celeration  command  a;  generated  by  the  guidance  system  is  exactly  the  same 
as  the  associated  centripetal  acceleration.  In  other  words,  the  guidance  logic 
chooses  a  reference  point  on  the  desired  path  at  a  distance  L\  ahead  of  the 
aircraft,  and  generates  the  acceleration  command  that  would  lead  the  vehicle 
to  hit  the  point  after  flying  a  circular  arc,  thus  leading  to  zero  steady-state 
error  for  a  circular  path.  As  shown  in  Chap.  3  of  [9],  the  performance  of  such 
a  lateral  guidance  law  for  circle  following  in  the  presence  of  wind  is  superior 
to  that  obtained  with  PD  or  PID  controllers.  With  the  nonlinear  guidance 
law  ai  =  sin  77,  the  vehicle  ground  speed  Vn  is  used  to  generate  the  accel¬ 
eration  command,  which  intrinsically  takes  into  account  the  inertial  velocity 
changes  due  to  the  wind  effects,  and  adapts  to  the  situation  accordingly. 


9. 2. 2. 2  Properties  Associated  with  the  Angle  g 

Equation  9.4  shows  that  the  direction  of  the  acceleration  a;  depends  on  the 
sign  of  g.  For  example,  if  the  reference  point  is  on  the  right  side  of  the 
direction  of  the  aircraft  velocity  vector,  then  the  aircraft  will  be  commanded 
to  accelerate  to  the  right,  and  finally  the  aircraft  will  tend  to  align  its  velocity 
vector  with  the  direction  of  L1. 

In  practice  the  distance  L\  is  fixed  to  a  certain  value,  and  two  cases  arise: 

•  If  the  aircraft  is  far  away  from  the  desired  path,  then  the  direction  of  Lx 
makes  a  large  angle  with  the  desired  path.  Therefore,  the  guidance  law 
chooses  the  reference  point  on  the  desired  trajectory  in  such  a  way  that 
the  aircraft  rotates  its  velocity  direction  to  approach  the  desired  path  at 
a  large  angle. 

•  If  the  aircraft  is  close  to  the  desired  path,  then  the  direction  of  L\  makes  a 
small  angle  with  the  desired  path.  Therefore,  the  guidance  law  chooses  the 
reference  point  on  the  desired  trajectory  in  such  a  way  that  the  aircraft 
rotates  its  velocity  direction  to  approach  the  desired  path  at  a  small  angle. 
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Since  the  angle  y  contains  the  information  about  the  upcoming  path,  this 
geometric  factor  has  the  effect  of  a  feedforward  control. 


9.2.3  Drawback  of  the  Method 

A  drawback  of  the  control  law  in  (9.4)  is  that  it  does  not  contain  any  element 
of  an  integral  control.  Therefore,  the  lateral  guidance  law  requires  to  be  pro¬ 
vided  with  an  unbiased  lateral  acceleration  measurement,  and  a  non-biased 
bank  angle  estimate. 


9.2.4  Selection  of  Lx 

The  design  parameter  in  the  lateral  guidance  logic  is  the  distance  Li  between 
the  vehicle  and  the  reference  point  as  shown  in  Fig.  9.3.  It  is  explained  in  [9] 
that  for  a  small  magnitude  of  rj,  the  guidance  formula  can  be  approximated 
in  terms  of  the  cross-track  error  y  as  follows: 

2V2  V„  (  Vn  \ 

al  =  -^-sinjy  «  2—  I  y+  —  y J  .  (9.5) 


Reference  point 


Fig.  9.3  Guidance  law  geometry 


Equation  9.5  shows  that  the  guidance  law  is  equivalent  to  a  PD  controller, 
in  which  the  ratio  between  the  vehicle  speed  Vn  and  the  distance  L\  is  an 
important  factor  that  behaves  as  the  gain  of  the  controller.  A  small  value  of 
Li  leads  to  a  high  control  gain  and  vice  versa.  The  control  gain  is  limited  by 
the  inner  loop  bank  control  bandwidth  (2—3  rad/s).  With  a  nominal  flight 
velocity  of  around  25  m/s,  the  distance  Li  has  been  chosen  to  be  Iq  =  150  m. 
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9.2.5  Path-planning  Objective 

The  main  objective  of  this  chapter  is  to  generate  on-line  an  appropriate  ref¬ 
erence  path  from  which  the  reference  point  P  can  be  selected  and  used  by 
the  lateral  guidance  control  law. 


9.3  Regular  Waypoint  Tracking 

The  regular  waypoint  tracking  algorithm  guides  the  aircraft  through  the  pre¬ 
defined  waypoints.  An  imaginary  segment  joins  two  consecutive  points,  and 
we  define  the  segment  k  to  be  the  segment  that  joins  the  waypoints  WPk 
and  WPk+i .  The  reference  point,  P,  from  the  guidance  law  presented  above, 
lies  on  this  segment  and  is  L\  distant  from  the  center  of  the  aircraft;  see  Fig. 
9.4. 


9.3.1  Computation  of  the  Reference  Point  P 


The  angle  of  the  segment  k  with  respect  to  North  is  defined  as  follows: 


V’sesffe)  =  tan- 


(WPk+1,E-WPk,E 
\WPk+ltN  -WPk,N 


(9.6) 


The  coordinates  of  the  current  location  of  the  center  of  the  aircraft  are  in 
the  North-East  plane  (A<v,  A/.;).  The  angles  x  and  A,  and  the  distance  d\ 
as  shown  in  Fig.  9.4  can  be  computed  as  follows: 


+  _i  (XE- 

x=ti"  (vyv 
A  -  Ixl 

di  =  y/(XE  -  WPk,E)2  +  (XN  -  WPk,N)2  .  (9.7) 

The  distance  between  WPk  and  the  reference  point  P  is  given  by 

[WPk,  P]  =  [WPk,  H]  +  [H,  P }  =  di  cos  A  +  yjL\-di 2  sin2  A  .  (9.8) 

Finally,  the  coordinates  of  the  reference  point  P  can  be  computed  with 


PN  =  WPk,N  +  [WPk,P]  cosj>seg{k)  , 

Pe  =  WPk,E  +  [WPk,  P]  sin  V’seg(fe)  •  (9.9) 


9.3  Regular  Waypoint  Tracking 
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Fig.  9.4  Regular  waypoint  tracking 


9.3.2  Logic  for  Segment  Switching 

As  the  aircraft  flies,  the  reference  point  P  also  moves  along  the  desired  tra¬ 
jectory,  which  consists  of  consecutive  segments.  Therefore,  the  guidance  al¬ 
gorithm  has  to  select  properly  the  current  segment  on  which  the  reference 
point  P  is  to  be  selected. 

Equation  9.10  gives  the  two  conditions  that  are  continuously  checked: 

d2  >  £i,  dicosA  <  \\WPk,WPk+1\\  .  (9.10) 

If  one  of  the  two  conditions  is  not  satisfied  anymore,  the  guidance  system 
has  to  select  a  reference  point  P  on  the  next  segment  [WPk+i,  WPk+2]-  On 
the  other  hand,  if  the  two  conditions  are  still  satisfied,  the  guidance  system 
computes  a  reference  point  P  on  the  segment  [WPk,  WPk+i].  In  that  case, 
the  path-planning  system  has  to  further  check  the  lateral  distance  between 
the  aircraft  and  the  segment  followed  in  order  to  make  sure  that  it  selects  a 
point  on  the  current  segment  that  is  L\  distant  from  the  aircraft. 
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Two  subcases  can  be  distinguished: 

•  If  |A|  >  7t/2,  the  aircraft  is  somewhere  behind  the  first  point  of  the  current 
segment.  In  that  case,  the  distance  Li  is  selected  as  Li  =  max(L-i  ,d\). 

•  If  |A|  <  7t/2,  the  guidance  algorithm  proceeds  in  checking  the  lateral  dis¬ 
tance  [C,H\  =  d\  sin  A.  If  [C,H]  >  Li,  then  the  distance  Li  is  assigned  a 
new  value  with  L\  =  1.1  x  [C,H\. 

In  this  way,  we  always  ensure  that  the  arm  L\  is  long  enough  to  intersect  the 

desired  trajectory,  so  that  a  reference  point  P  always  exists. 


9.3.3  Computation  of  the  Roll  Angle  Command  </>COTn. 

The  direction  of  the  aircraft’s  ground  speed  is  computed  with 

=  tan"1  e  [— tt;  tt]  .  (9.11) 

The  lateral  guidance  control  law  needs  the  angle  77,  which  is  computed  as 
follows  (see  Fig.  9.6): 

77  =  Q  —  ip  , 

^ <9i2> 

In  practice,  we  want  the  angle  rj  to  be  in  the  range  [ — 7r;  7t]  ,  therefore  the 
following  code  implementation  is  used 

while  (77  >  7r)  77  =  77  —  27r  , 
while  (77  <  —7 r)  rj  =  r]  +  2n  .  (9.13) 

Once  the  angle  77  is  in  the  range  [ — 7r;  7t],  the  angle  T)  is  further  limited 
(saturated  if  needed)  to  be  in  the  range  [ — 7t/2;  7t/2].  The  reason  for  this  is  that 
when  the  angle  77  becomes  large  and  approaches  ir/2  (or  —n/2,  respectively) 
we  want  the  aircraft  to  bank  to  the  right  (or  to  the  left,  respectively)  as 
much  as  is  possible  to  quickly  come  closer  again  to  the  reference  trajectory. 
Indeed,  since  the  angle  77  is  used  in  the  roll  angle  command  with  <j>COm  = 
2V%  sm.r]/{L\g),  if  77  exceeded  7r/2  (— 7t/2)  in  the  range  [7r/2;  7r]  ([— 7t/2;  — 7t]) 
the  term  sin  77  would  decrease  in  amplitude,  which  means  that  the  commanded 
roll  angle  also  decreases,  and  the  aircraft  would  come  back  more  slowly  to 
the  reference  trajectory,  which  is  not  desired.  Finally,  the  control  signal  for 
the  roll  angle  (j)COm  is  saturated  within  the  range  [—(j>max',  fmax}- 
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9.4  Altitude  Guidance  Law 

The  altitude  control  signal  is  hc  and  is  computed  as  follows: 

hc  =  WPktD  +  'ydi  cos  A  , 

with  the  desired  flight  path  angle  defined  as  follows: 

WPk+1,D  -  WPk,D 

y/( WPk+1,N  ~  WPk,N)2  +  ( WPk+1,E  -  WPk,Ef 

with  A  and  di  defined  in  (9.7). 

Figure  9.7  shows  the  aircraft  flying  from  waypoint  WPk  to  WPk+\.  The 
desired  altitude  is  hc  computed  as  a  function  of  the  aircraft’s  ground  position. 
The  aircraft’s  current  altitude  is  Xjj ,  which  is  obviously  too  high  compared 
with  hc  in  Fig.  9.7,  and  the  altitude  controller  of  Section  7.5  must  correct 
this  altitude  error. 


(9.14) 

(9.15) 


Remarks: 

•  Equations  9.14  and  9.15  are  used  to  generate  the  altitude  reference  signal 
in  the  normal  waypoint  tracking  mode. 

•  In  the  modes  evasion  maneuver  (mode  1)  or  circle  tracking  (mode  2),  the 
altitude  reference  signal  is  kept  at  the  value  where  it  was  before  the  guid¬ 
ance  system  entered  one  of  these  modes.  Indeed,  in  mode  1  or  2  the  aircraft 
takes  a  path  that  is  not  known  in  advance  and  for  which  no  altitude  may 
be  specified.  Once  the  guidance  system  switches  back  to  normal  waypoint 
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tracking  mode  (mode  0),  the  altitude  control  signal  is  again  constantly 
recomputed. 


9.5  NFZ  and  Obstacles 

Before  the  flight,  the  location  of  any  known  NFZ  are  stored  in  the  memory  of 
the  autopilot.  If  the  UAV  is  equipped  with  scanning  sensors  that  can  detect 
pop-up  obstacles,  our  path-planning  system  will  recompute  on  the  fly  a  new 
trajectory.  It  determines  whether  an  NFZ  or  an  obstacle  interferes  with  the 
planned  path  by  using  an  imaginary  “detection  line”  of  length  Rla  in  front 
of  the  aircraft,  as  shown  in  Fig.  9.8.  The  distance  Rla  defines  the  so-called 
“look-ahead  distance”.  If  any  part  of  this  detection  line  penetrates  an  NFZ 
or  an  obstacle,  avoidance  action  (mode  1)  is  immediately  taken  as  described 
in  the  next  section. 


9. 5. 1  Definition  of  an  NFZ 

An  NFZ  is  any  airspace  in  which  an  aircraft  is  not  permitted  to  fly.  This 
airspace  can  be  of  any  arbitrary  shape.  However,  in  order  to  simplify  the 
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4 


Fig.  9.8  NFZ  and  look-ahead  distance  Rla 


guidance  algorithm,  two  conditions  are  imposed  on  how  the  NFZ  is  repre¬ 
sented. 

First,  the  vertical  limits  of  the  NFZ  are  not  considered  such  that  the 
NFZ  is  essentially  a  2D  surface.  The  aircraft  is  not  allowed  to  pass  over  the 
NFZ.  Second,  the  shape  of  the  NFZ  is  chosen  to  be  a  circle.  In  this  way,  the 
avoidance  maneuver  can  be  an  arc  of  a  circle  in  order  to  benefit  from  the 
guidance  control  law  especially  suited  to  track  circles,  which  are  described  by 
only  two  parameters,  namely  their  center  and  their  radius. 

Although  this  chapter  discusses  the  avoidance  of  one  circular  NFZ  only,  the 
algorithm  can  be  extended  to  multiple  NFZs  with  some  simple  modifications. 
Also,  a  complex  NFZ  shape  can  be  represented  by  multiple  circles. 


9.5.2  Choice  of  an  Appropriate  Look-ahead  Distance 
Rla 

The  distance  Rla  defines  the  so-called  “look-ahead  distance”.  If  any  part 
of  this  detection  line  penetrates  an  NFZ  or  an  obstacle,  avoidance  action  is 
immediately  taken  as  described  in  the  next  section. 

The  look-ahead  distance  Rla  is  chosen  such  that  the  aircraft  will  fly  an 
arc  that  stays  just  outside  the  NFZ  and  start  the  evasion  maneuver  as  late 
as  possible.  The  value  of  Rla  depends  on  the  radius  Rnfz  of  the  NFZ, 
the  ground  speed  of  the  aircraft  Vn,  and  the  maximum  bank  angle  of  the 
aircraft  (pmax-  Given  these  parameters,  and  assuming  a  coordinated  turn,  the 
minimum  turn  radius  that  the  aircraft  can  fly  is  given  by 

Vf 

<jitan(</>TOaa:)  ’ 

yjv*  +  Vj  .  (9.16) 

The  subscript  n  attached  to  the  ground  speed  of  the  aircraft  Vn  indicates 
that  the  speed  is  taken  in  the  local  navigation  frame. 
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In  the  case  of  an  NFZ  with  infinite  radius,  the  aircraft  would  have  to  make 
a  90°  turn,  in  which  case  Rla, min  =  Rmin-  For  any  NFZ  with  a  finite  radius, 
the  aircraft  has  to  turn  less  than  90°  to  avoid  it.  Figure  9.9  shows  the  situation 
where  the  aircraft  is  at  the  point  where  it  begins  its  turn  and  is  guided  so 
that  its  path  becomes  tangent  to  the  edge  of  the  NFZ.  A  triangle  can  be  set 
up  with  vertices  at  the  center  of  the  NFZ,  at  the  center  of  the  aircraft,  and 
at  a  point  Rmin  off  the  right  wing-tip.  The  minimum  look-ahead  distance 
Rla, min  is  constructed  using  the  Pythagorean  theorem 

(■ RLA,min  +  RnFZ )2  +  R-min  =  ( Rmin  +  RnFz)2  ■  (9-17) 

By  expanding  both  sides  and  regrouping  the  terms,  we  obtain  the  following 
equation,  where  the  distance  Rla, min  is  the  unknown  variable 

R^A,min  +  2 RLA,minRNFZ  ~  2 RminRNFZ  =  0  .  (9.18) 

Equation  9.18  admits  two  real  solutions  of  which  we  only  keep  the  positive 
one 

RLA,min  =  V RnFZ\J RnFZ  +  2Rmin  ~  RnFZ  ■  (9.19) 


Rla 


To  obtain  the  final  value  for  Rla,  compensation  must  be  made  for  the 
delay  needed  to  initiate  the  turn  and  to  bank  to  (j>max ■  The  assumption  is 
made  that  while  the  aircraft  is  initiating  the  turn,  it  continues  to  fly  level, 
and  then  as  soon  as  it  reaches  4>max  it  takes  a  minimum  radius  turn.  The 
characteristic  time  rrou  to  roll  to  <i>max  can  be  multiplied  by  the  aircraft’s 
speed  to  obtain  the  distance  the  aircraft  will  travel  during  this  delay,  which 


214  9  Reconfigurable  Guidance  System 

is  added  to  Rla, min ■  The  resulting  look-ahead  distance  is 

RLA  =  RLA,min  +  VnTroll  .  (9.20) 


9.6  Detection  of  the  NFZ 

As  mentioned  before,  the  algorithm  monitors  an  “imaginary”  line  of  length 
RLa  ahead  of  the  aircraft,  and  checks  if  it  penetrates  any  NFZ  or  obsta¬ 
cle.  First,  the  distance  Dnfz  from  the  aircraft  to  the  center  of  the  NFZ  is 
calculated;  see  Fig.  9.10: 

dnfz  =  s/{nfzn  -  xN)2  +  {NFZE  -  xE)2  .  (9.21) 


Fig.  9.10  Geometry  of  the  NFZ  approach 

If  the  condition 

Dnfz  <  Rnfz  +  Rla  (9.22) 

is  satisfied,  where  the  length  Rnfz  is  the  radius  of  the  NFZ,  then  the  aircraft 
is  considered  to  be  within  range  of  the  NFZ,  which  means  that  the  aircraft 
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is  in  the  light-gray  area  in  Fig.  9.10.  In  this  case,  a  further  check  is  made  to 
see  if  a  part  of  the  detection  line  is  touching  the  NFZ. 

There  are  two  possible  cases  shown  in  Figs.  9.11  and  9.12  depending  on 
the  orientation  of  the  aircraft.  The  distances  Dnfz  and  Ft, la  are  known.  We 
also  define  the  angle  V’jvfz  between  the  North  direction  at  the  center  of  the 
aircraft  and  the  segment  of  length  Dnfz  as  follows: 


IpNFZ  = 


FNFZe-Xe\ 

\NFZn-Xn) 


(9.23) 


A  pair  of  triangles  is  created  as  shown  in  Fig.  9.11.  The  length  of  the  edges 
y  and  a  can  easily  be  calculated,  using 


=  \ipNFZ~ip\ ,  €  [— 7r;  7 r]  , 

y  =  Dnfz  ■  sin  A  , 

a  =  Dnfz  ■  cos  A  .  (9.24) 


Case  1:  a  <  Rla 

Case  1  applies  if  a  <  Rla-  The  limiting  case  occurs  when  edge  a  is  tangent 
to  the  NFZ.  Therefore,  y  will  have  a  length  equal  to  Rnfz ■  Thus,  the  NFZ 
touches  the  detection  line  if  y  <  Rnfz ■ 
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Fig.  9.12  Diagram  of  NFZ  detection  algorithm,  Case  2  (not  detected) 


Case  2:  a  >  RLa 


Case  2  applies  if  a  >  Rla-  The  limiting  case  occurs  when  the  end  of  the 
detection  line  is  on  the  edge  of  the  NFZ.  This  can  be  checked  by  comparing 
the  length  of  edge  x  with  the  radius  of  the  NFZ,  so  that  the  NFZ  touches 
the  detection  line  if  x  <  Rnfz ,  where  x  =  y/y2  +  (a  —  Rla)2- 

The  check  for  Case  1  or  Case  2  is  only  done  if  A  is  less  than  or  equal 
to  90°.  If  A  is  greater  than  90°,  then  the  center  of  the  NFZ  lies  behind  the 
aircraft  and  no  action  is  taken.  Figure  9.13  shows  the  diagram  of  the  NFZ 
detection  algorithm. 


9.7  NFZ  Avoidance  Algorithm 

The  NFZ  avoidance  algorithm  guides  the  aircraft  around  any  NFZ  that  the 
aircraft  encounters.  The  avoidance  method  is  designed  to  be  simple  to  imple¬ 
ment  while  allowing  the  aircraft  to  reach  waypoints  close  to  the  edge  of  the 
NFZ. 
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9.7.1  On-line  Selection  of  an  Avoidance  Path  Template 

One  key  feature  of  this  avoidance  method  is  the  on-line  generation  of  a  cir¬ 
cular  arc  around  the  NFZ  as  a  reference  path,  drawn  as  a  dashed  line  in  Fig. 
9.14.  Such  a  path  minimizes  the  distance  the  aircraft  flies  to  avoid  the  NFZ. 
Moreover,  we  saw  at  the  beginning  of  this  chapter  that  the  lateral  guidance 
control  law  is  particularly  efficient  in  tracking  circles. 

Furthermore,  choosing  the  reference  path  to  be  circular  allows  the  template 
path  to  be  easily  defined  in  relationship  to  the  NFZ  dimensions.  It  is  indeed 
defined  by  the  center  of  the  NFZ  and  a  path  radius,  Ri,  which  is  simply  the 
NFZ  radius  plus  a  safety  margin.  The  aircraft  follows  this  path  until  it  is  able 
to  continue  towards  the  next  waypoint  in  a  straight  line  and  without  passing 
through  the  NFZ. 


Fig.  9.14  Circular  template  path,  waypoint  tracking  and  reconfiguration 


9.7  NFZ  Avoidance  Algorithm 

9.7.2  Entering  the  Circular  Path  Template 
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As  soon  as  the  obstacle  is  detected  to  be  an  immediate  threat  to  the  aircraft  at 
point  Ti,  the  guidance  system  begins  an  evasion  maneuver  with  the  minimum 
turn  radius  possible  to  reach  tangentially  the  template  path  at  point  T 2;  see 
Fig.  9.14. 


9.7.3  Choice  of  the  Avoidance  Side 

Whether  the  guidance  algorithm  chooses  to  go  left  or  right  around  the  NFZ 
is  determined  by  which  side  of  the  NFZ  center  the  aircraft  is  already  flying 
towards.  If  the  aircraft’s  velocity  vector  is  pointing  to  the  right  of  the  NFZ 
center,  then  the  aircraft  will  fly  around  the  NFZ  on  the  right-hand  side.  If  the 
velocity  vector  is  pointing  to  the  left-hand  side,  then  the  aircraft  flies  around 
the  NFZ  on  the  left-hand  side.  A  circular  NFZ  makes  this  decision  easy. 

In  practice,  we  compute  A  =  iPnfz  —  ip  [ — 7r;  7r]),  and  if  A  >  0  then 

the  aircraft  flies  on  the  left-hand  side  of  the  NFZ,  otherwise  it  evades  on  the 
right-hand  side;  see  Fig.  9.15. 


Fig.  9.15  NFZ  evasion  side 
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9.7.4  Generating  the  Template  Path 

Once  the  evasion  maneuver  is  complete,  the  extremity  of  the  monitoring  line 
lies  outside  the  NFZ.  If  the  next  waypoint  to  reach  is  obstructed  by  the  NFZ, 
the  guidance  system  guides  the  aircraft  around  the  NFZ  until  there  is  a  clear 
line  of  sight  to  the  next  valid  waypoint.  In  this  case,  the  guidance  system  has 


North 


Fig.  9.16  Circular  template  path  generation 


to  guide  the  aircraft  to  a  point  P  that  is  on  the  circle  R\  and  at  a  distance 
L[  ahead  of  the  aircraft.  Using  the  law  of  cosines,  the  angle  (3  (Fig.  9.16) 
between  the  segment  that  joins  the  center  of  the  aircraft  to  the  center  of  the 
NFZ  and  the  segment  from  the  center  of  the  aircraft  to  the  new  reference 
point  P  is  expressed  as 


9  =  arccos 


2 Dnfz  U] 


(9.25) 
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In  order  to  make  sure  that  the  generated  circle  to  avoid  the  NFZ  is  a  feasible 
path  for  the  aircraft,  the  radius  R\  is  selected  as 

Ri  =  max  ( Rmin ,  Rnfz  +  safety  value)  .  (9.26) 

The  coordinates  of  the  reference  point  P  can  then  be  computed  as  follows: 

•  If  the  NFZ  is  to  be  avoided  on  the  right-hand  side,  we  compute  \P, avoid,  = 

i’NFZ  +  P 

•  If  the  NFZ  is  to  be  avoided  on  the  left-hand  side,  we  compute  Pavoid  = 

IpNFZ  ~  P 

and  finally 

Pn  =  Xjy  +  Li  cos(\Pavoid)  , 

PE  =  XE  +  Li  sin {Pavoid)  .  (9.27) 


9.7.5  Leaving  the  Circular  Path  Template 

The  aircraft  follows  the  circular  path  until  it  is  able  to  continue  towards  the 
next  waypoint  in  a  straight  line  and  without  passing  through  the  NFZ.  The 
point  at  which  the  guidance  algorithm  transitions  back  to  normal  guidance 
(mode  0)  towards  the  next  waypoint  is  T3  in  Fig.  9.14,  and  it  occurs  when 
there  is  a  clear  line  of  sight  from  the  aircraft’s  current  position  to  the  next 
waypoint. 

The  next  waypoint  has  to  be  reachable,  which  means  that  it  should  lie 
outside  a  no-fly  area.  Therefore,  while  initiating  the  evasion  maneuver,  the 
guidance  system  analyzes  the  waypoint  list  and  checks  if  the  waypoint  WPk+i 
that  was  tracked  before  the  evasion  maneuver  is  still  a  reachable  point.  The 
following  condition  must  hold: 

dNFZ/WPk+ 1  =  y/(NFZN  -  WPk+1,N)2  +  ( NFZe  -  WPk+1,E)2  >  Ri  ■ 

(9.28) 

If  it  turns  out  that  this  waypoint  is  not  reachable,  the  guidance  system 
selects  the  next  waypoint  in  the  list,  and  so  on.  Figure  9.14  illustrates  the 
waypoint  reconfiguration.  The  aircraft  makes  a  left  turn  at  waypoint  WP2 
in  the  direction  of  WP3.  Then  the  NFZ  is  detected  and  the  guidance  system 
guides  the  aircraft  around  the  obstacle.  The  unreachable  waypoint  WP3  is 
discarded  and  instead  WP4  becomes  the  next  target. 
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evade  =  +2,  circle  tracking  right 


Fig.  9.17  Template  path  generation  diagr 


9.8  Simulation 
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When  should  the  guidance  transition  X3  shown  in  Fig.  9.14  occur?  A  sim¬ 
ple  criterion  is  to  monitor  the  angle  A  as  shown  in  Fig.  9.14,  which  is  the 
angle  between  the  segment  made  by  the  aircraft’s  center  and  the  next  valid 
waypoint,  and  the  segment  made  by  the  aircraft’s  center  and  the  center  of 
the  NFZ.  As  soon  as  |A|  >  tt/2,  then  there  is  a  clear  line  of  sight  to  the  next 
valid  waypoint.  The  angle  A  can  be  expressed  simply  as  follows: 

A  =  6»!  -  02  S  [—7 r;  n\  ,  (9.29) 


with 


61 

O2 


-1  (  WPk+ 1.. 


\WPk+1>N-XNJ  ’ 

,  ,  _1fNFZE-XE\ 

^ =  tan  [nfzn-xn) 


(9.30) 


The  decision  steps  for  the  next  valid  waypoint  are  summarized  in  Fig.  9.18. 


9. 7. 6  Properties  of  the  Guidance  Schedule 

The  guidance  schedule  presented  here  has  several  desirable  properties.  It 
attempts  to  minimize  the  number  of  waypoints  that  are  unreachable  by  ini¬ 
tiating  the  avoidance  maneuver  as  late  as  possible.  It  does  not  require  any 
complex  logic  to  decide  how  to  avoid  the  NFZ.  Finally,  it  minimizes  the  dis¬ 
tance  and  time  needed  to  return  to  the  original  flight  path.  It  does  this  by 
flying  directly  to  the  next  waypoint  as  soon  as  is  safely  possible. 


9.8  Simulation 

9.8.1  Simulation  Set-up 

Simulations  were  done  using  the  nonlinear  six  degree-of-freedom  computer 
model  of  a  radio  controlled  aerobatic  aircraft  described  in  Chap.  3.  The  flight 
controllers  used  are  described  in  Chaps.  7  and  8.  The  airspeed,  the  altitude, 
and  the  sideslip  angle  are  kept  constant. 

Three  similar  scenarios  were  simulated,  with  the  results  presented  below. 
In  all  scenarios,  the  aircraft  is  following  a  desired  path  that  passes  through  an 
NFZ.  The  simulation  was  done  with  a  maximum  bank  angle  of  (j>max  —  30°. 
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Fig.  9.18  Next  valid  point  and  clear  line-of-sight  determinatk 


9.8  Simulation 
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9.8.2  Simulation  Results 
9.8.2. 1  No  Wind 

This  first  scenario,  shown  in  Fig.  9.19,  highlights  the  basic  response  of  the 
aircraft  to  an  NFZ  blocking  its  path.  The  aircraft  begins  South  of  the  NFZ 
and  flies  North  along  the  desired  path  defined  by  the  waypoints  1  to  5  and 
returns  back  to  the  runway.  The  desired  path  passes  through  an  NFZ,  but 
the  aircraft  deviates  around  it  before  returning  to  the  desired  path.  The 
simulation  was  run  at  three  different  flight  speeds,  15,  30,  and  45  m/s.  It  can 
be  seen  that  the  aircraft  begins  its  turn  much  later  when  flying  at  15  m/s 
than  when  flying  at  45  m/s.  The  airplane  stays  outside  the  NFZ  at  all  three 
speeds. 


9. 8. 2. 2  With  Wind 

This  second  scenario,  shown  in  Fig.  9.20,  highlights  the  response  of  the  air¬ 
craft  in  wind  conditions.  The  desired  path  remains  the  same  as  in  the  first 
scenario.  The  path  taken  by  the  aircraft  without  wind  and  with  wind  are 
shown  for  comparison.  The  aircraft  is  flying  at  a  nominal  airspeed  of  30  m/s. 

A  first  flight  is  made  with  a  6  m/s  crosswind  blowing  from  West  to  East. 
In  this  case,  the  path  followed  by  the  aircraft  is  almost  identical  to  the  one 
without  wind. 

Another  flight  simulation  is  made  with  wind  blowing  from  South  to  North 
with  a  speed  of  6  m/s.  The  trajectory  in  the  latter  windy  condition  differs 
from  the  the  nominal  track  (without  wind)  in  the  two  turns  that  avoid  the 
obstacle,  where  there  is  a  maximum  difference  of  20  m.  This  is  due  to  the  fact 
that  the  wind  speed  adds  to  the  airspeed  resulting  in  a  higher  ground  speed 
Vn  than  the  one  obtained  with  no  wind.  Thus,  the  evasion  maneuver  starts 
earlier  when  the  aircraft  first  encounters  the  obstacle  in  between  points  1  and 
2.  In  between  points  4  and  5,  the  wind  is  facing  the  aircraft,  and  therefore, 
the  ground  speed  is  lower  than  the  one  obtained  with  no  wind.  This  explains 
why  the  evasion  maneuver  starts  later. 

In  both  cases,  the  NFZ  is  avoided.  After  the  obstacle  has  been  avoided, 
the  guidance  system  resumes  normal  waypoint  tracking.  As  expected  from 
the  lateral  guidance  law  described  in  Sect.  9.2  there  is  no  steady-state  error 
when  flying  the  circular  arc  around  the  NFZ  as  shown  in  Fig.  9.20. 
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9.9  Conclusions 

This  chapter  presented  a  guidance  algorithm  that  combines  simplicity  of  im¬ 
plementation  and  ability  to  avoid  an  NFZ.  The  algorithm  successfully  demon¬ 
strated  in  simulation  its  ability  to  guide  the  aircraft  around  the  NFZ  and  then 
to  resume  flying  along  the  desired  path.  The  guidance  system  intrinsically 
takes  into  account  the  wind  condition  via  the  ground  speed  of  the  aircraft. 
Finally,  the  method  is  computationally  efficient. 
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